package pteam;

import java.util.*;
import java.io.*;

import simulator.Actor;
import simulator.Robot;
import simulator.Sensor;
import simulator.Simulator;

public class SimpleNavigator implements Actor {

    public static boolean adjusting;
    public static final int lfront = 1;
    public static final int rfront = 0;
    public static final int lback = 2;
    public static final int rback = 3;
    public double[] sensors = new double[5];
    public static final double tol = 1.;
    public static boolean canAct = true;

    public SimpleNavigator() {
        adjusting = false;
    }

    public String getMaze() {
        return "Testing"; // TODO(kroo): some one needs to do something asdfasd
    }

    public void updateSensors(Sensor[] s) {
        int i = 0;
        for (int z = 0; z < s.length; z++) {
            Sensor sensor = s[z];
            double val = sensor.getState()[0];
            if (sensor.getName().equals("RangeFinder")) {
                sensors[i] = val;
                i++;
//  Simulator.out.println("RangeFinder found (val = " + val + ")");
            } else {
//  Simulator.out.println("another type of sensor found and ignored: " + sensor.getName());
            }
        }
    }

    public void act(Robot r) {
        if (!canAct) {
            return;
        }
        Simulator.out.println("            simplenav acting");
        try {
        } catch (Exception e) {
        }
        if (noVals()) {
            // Simulator.out.println("no vals");
            // do nothing
        } else if (atTurn() && false) {
            boolean left = sensors[lfront] > sensors[rfront];


            adjusting = false;

        } else if (pointingR()) {
//      Simulator.out.println("pointing R()");
            adjusting = true;
            r.turnLeft();
            r.waitAngle(1);
        } else if (pointingL()) {
//      Simulator.out.println("pointing L()");
            adjusting = true;
            r.turnRight();
            r.waitAngle(1);
        } else if (false) {
            return;
        } else if (pointingCenter()) {
//      Simulator.out.println("centered");
            good(r);
        } else if (onLeftSide()) {
            adjusting = true;
//      Simulator.out.println("onleftside");
            r.turnRight();
            r.waitAngle(5);
            r.goForward();
            r.waitDistance(5);
            r.turnLeft();
            r.waitAngle(5);
            // turn right, drive until in middle, turn to orient straight
        } else if (onRightSide()) {
            adjusting = true;
//      Simulator.out.println("onrightside");
            r.turnLeft();
            r.waitAngle(5);
            r.goForward();
            r.waitDistance(5);
            r.turnRight();
            r.waitAngle(5);
        } else {
            adjusting = false;
//      Simulator.out.println("ESSPLODE");
            r.goForward();
            r.waitDistance(5);
            // WHOT
        }
    }

    public boolean pointingR() {
        double tol = 3;
        double leftside = sensors[lback] - sensors[lfront]; // front > back: pointing right
        double rightside = sensors[rfront] - sensors[rback]; //
        boolean result = (leftside <= -tol && rightside <= -tol);
//    Simulator.out.println((result?"(pointing right) ":"(not pointing right) ") + sensors[lfront] + " " + sensors[lback] + " " + sensors[rfront] + " " + sensors[rback] + " " +leftside + " " + rightside);
        return result;
    }

    public boolean pointingL() {
        double tol = 3;
        double leftside = sensors[lback] - sensors[lfront];
        double rightside = sensors[rfront] - sensors[rback];
        boolean result = (leftside >= tol && rightside >= tol);
        //  Simulator.out.println((result?"(pointing left) ":"(not pointing left) ") + sensors[lfront] + " " + sensors[lback] + " " + sensors[rfront] + " " sensors[rback] + " " +leftside + " " + rightside);
        return result;
    }

    public boolean pointingCenter() {
        double tol = 2;
        double backside = sensors[lback] - sensors[rback];
        double frontside = sensors[lfront] - sensors[rfront];
        boolean result = (Math.abs(backside) <= tol && Math.abs(frontside) <= tol);
//    Simulator.out.println((result?"(centered) ":"(not centered) ") + sensors[lfront] + " " + sensors[lback] + " " + sensors[rfront] + " " + sensors[rback] + " " +backside + " " + frontside);
        return result;
    }

    public boolean onRightSide() {
        double tol = 2;
        double backside = sensors[lback] - sensors[rback];
        double frontside = sensors[lfront] - sensors[rfront];
        boolean result = (backside > tol && frontside > tol);
//    Simulator.out.println(result?"(right side) ":"(not right side) " + sensors[lfront] + " " + sensors[lback] + " " + sensors[rfront] + " " + sensors[rback] + " " +backside + " " + frontside);
        return result;
    }

    public boolean onLeftSide() {
        double tol = 2;
        double backside = sensors[lback] - sensors[rback];
        double frontside = sensors[lfront] - sensors[rfront];
        boolean result = (backside < -tol && frontside < -tol);
//    Simulator.out.println(result?"(left side) ":"(not left side) " + sensors[lfront] + " " + sensors[lback] + " " + sensors[rfront] + " " + sensors[rback] + " " +backside + " " + frontside);
        return result;
    }

    /**
     * Everything is rose-y, continue going straight.
     */
    public void good(Robot r) {
        r.goForward();
        r.waitDistance(1);
    }

    public boolean goingToLCrash() {
        return (sensors[lfront] < tol * 2);
    }

    public boolean goingToRCrash() {
        return (sensors[rfront] < tol * 2);
    }

    public boolean noVals() {
        //Simulator.out.println("NO VALS: " + sensors[rfront] + " " + sensors[lfront]);
        if (sensors[rfront] == 0
                && sensors[lfront] == 0
                && sensors[rback] == 0
                && sensors[lback] == 0) {
            return true;
        } else {
            return false;
        }
    }

    public boolean goingToCenterL() { //going towards the left
        return (sensors[rfront] > sensors[rback] && sensors[lfront] > sensors[rfront]);

    }

    public boolean goingToCenterR() { //going towards the right
        return (sensors[lfront] > sensors[lback] && sensors[rfront] > sensors[lfront]);
    }

    public boolean atCenter() { //note: checks if bot is in center, not if bot is pointing forwards
        return (Math.abs(sensors[rfront] + sensors[rback] - sensors[lfront] - sensors[lback]) < tol);

    }

    public boolean atTurn() {
        double tol = .5;
        double rf = sensors[rfront] + 1,
                lb = sensors[lback] + 1,
                lf = sensors[lfront] + 1,
                rb = sensors[rback] + 1;

        double ratio1 = rf / lb;
        double ratio2 = rb / lf;
        boolean result = !(Math.abs(ratio1 - ratio2) < tol);

//    Simulator.out.println((result?"(at turn) ":"(not at turn) ") + sensors[lfront] + " " + sensors[lback] + " " + sensors[rfront] + " " + sensors[rback] + " " + ratio1 + " " + ratio2);

        return result;
    }
}
